import java.util.Arrays;

import lejos.nxt.Button;
import lejos.nxt.ButtonListener;
import lejos.nxt.LCD;
import lejos.nxt.SensorPort;
import lejos.nxt.SoundSensor;


public class PartyFollower {
	private static SoundSensor micLeft;
	private static SoundSensor micRight;
	private static int offset = 0;
	private static final int calibrateSize = 20;
	private static final int cruiseSpeed = 70;
	private static DataLogger logger;
	public static void main(String[] args) throws InterruptedException {
		micLeft = new SoundSensor(SensorPort.S2);
		micRight = new SoundSensor(SensorPort.S1);
		
		logger = new DataLogger("partyLog.csv", false);
		logger.start();
		
		Button.ESCAPE.addButtonListener(new ButtonListener() {
			
			@Override
			public void buttonReleased(Button b) {
				// TODO Auto-generated method stub
				
			}
			
			@Override
			public void buttonPressed(Button b) {
				// TODO Auto-generated method stub
				logger.close();
				System.exit(0);
			}
		});
		
		LCD.drawString("Calibrating...", 0, 0);
		
		calibrate();
		
		LCD.clear();
		
		while(true)
		{
			int difference = (int) ((getLeftSound() - getRightSound()));
			
			//Used for data logger
			logger.writeSample(String.valueOf(getLeftSound()) + logger.divider + String.valueOf(getRightSound()));
			
			Car.forward(normalize(getLeftSound())-difference*2, normalize(getRightSound())+difference*2);
			Thread.sleep(10);
		}
	}
	
	private static void calibrate() throws InterruptedException
	{
		int[] leftSensor = new int[calibrateSize];
		int[] rightSensor = new int[calibrateSize];
		
		Car.forward(cruiseSpeed, cruiseSpeed);
		
		for(int i=0; i<calibrateSize; i++)
		{
			leftSensor[i] = micLeft.readValue();
			rightSensor[i] = micRight.readValue();
			Thread.sleep(50);
		}
		
		Car.stop();
		
		Arrays.sort(leftSensor);
		Arrays.sort(rightSensor);
		
		int medianLeft = leftSensor[calibrateSize/2];
		int medianRight = rightSensor[calibrateSize/2];
		
		offset = medianRight - medianLeft;
	}
	
	private static int getLeftSound()
	{
		return micLeft.readValue() + offset;
	}
	
	private static int getRightSound()
	{
		return micRight.readValue();
	}
	
	private static int normalize(int value)
	{
		return (int)Math.round(60 + value * 0.4);
	}

}
